/*
 **	Skeleton.cpp

 Program:   
 Module:    
 Language:  C++
 Date:      
 Version:   

 **      Created: Nov 23 2007.
 **	
 **	purpose:   
 */

#ifndef __SKELETON_CPP__
#define __SKELETON_CPP__

#include "Skeleton.h"

using namespace std;

// Constructor parsing skeleton and motion files (ASF/AMC).
// @param skeletonFile name of skeleton file (ASF)
// @param motionFile name of motion file (AMC)
Skeleton::Skeleton(string skeletonFile, string motionFile) {
	//    minPoint = make_vector(1000, 1000, 1000); 
	//    maxPoint = make_vector(-1000, -1000, -1000);
	cout << "loading skeleton..." << endl;
	loadASF(skeletonFile);
	cout << "skeleton file loaded!" << endl;
	initial_pose = buildPose(position, bones);
	//motion.push_back(initial_pose);
	//skeletons.push_back(bones);
	cout << "loading motion data..." << endl;
	minPoint = maxPoint = position;
	loadAMC(motionFile);
	cout << "motion data loaded! " << motion.size() << " frames" << endl;
}

// Constructor parsing bvh file
// @param bvhFile name of file (BVH)
Skeleton::Skeleton(string bvhFile){
	cout << "loading bvh..." << endl;
	loadBVH(bvhFile);
	cout << "bvh file loaded!" << endl;
	//initial_pose = buildPose(position, bones);
	//~ motion.push_back(initial_pose);
}

// Build a pose corresponding to current skeleton.
// @param root_position actual position of root
// @param current_skeleton given skeleton
// @return The pose constructed
Skeleton::Pose Skeleton::buildPose(Vector3d root_position, map<string, Bone> current_skeleton) {

	Pose pose;
	map<string, string> joint_conex;
	Joints jt = createJoints();

	string current_bone = "root";
	Vector3d current_position;
	list<string> queue;
	vector<string> current_sons;

	queue.push_back(current_bone);
	while (!queue.empty()) {
		current_bone = queue.front();
		queue.pop_front();
		if (current_bone != "root") {
			current_position = jt[current_bone] + current_skeleton[current_bone].lenXdir();
		} else {
			current_position = root_position;
		}

		jt[current_bone] = current_position;

		// Refreshing queue
		current_sons = hierarchy[current_bone];
		for (unsigned int i = 0; i < current_sons.size(); i++) {
			jt[current_sons[i]] += current_position;
			joint_conex[current_sons[i]] = current_bone;
			queue.push_back(current_sons[i]);
		}
	}

	joints.push_back(jt);

	for (map<string, string>::iterator it = joint_conex.begin(); it != joint_conex.end(); it++) {
		Edge edge;
		edge.first = jt[it->second];
		edge.second = jt[it->first];

		minPoint.x = minPoint.x < edge.second.x ? minPoint.x : edge.second.x;
		minPoint.y = minPoint.y < edge.second.y ? minPoint.y : edge.second.y;
		minPoint.z = minPoint.z < edge.second.z ? minPoint.z : edge.second.z;
		maxPoint.x = maxPoint.x > edge.second.x ? maxPoint.x : edge.second.x;
		maxPoint.y = maxPoint.y > edge.second.y ? maxPoint.y : edge.second.y;
		maxPoint.z = maxPoint.z > edge.second.z ? maxPoint.z : edge.second.z;

		pose[it->first] = edge;
	};

	return pose;
}

// Create an default joints map.
// @return joints structure
map<string, Vector3d> Skeleton::createJoints() {
	map<string, Vector3d> jt;
	jt["root"] = make_vector(0.0, 0.0, 0.0);
	for (map<string, Bone>::iterator it = bones.begin(); it != bones.end(); it++)
		jt[it->second.getName()] = make_vector(0.0, 0.0, 0.0);
	return jt;
}

// Rotate around generic axis.
// @param angle given rotation angle
// @param vec given vector for rotation
// @param axis given rotation axis
// @return rotated vector
Vector3d Skeleton::rotate(double angle, Vector3d vec, Vector3d axis) {
	if (is_degree)
		angle = angle * 3.1415 / 180.0;

	axis = normalize(axis);
	double cos_a = cos(angle);
	double sin_a = sin(angle);

	Vector3d vp = vec * cos_a + cross_product(axis, vec) * sin_a + (axis * vec) * axis * (1.0 - cos_a);
	return vp;
}

// Parse command string to enum value
// @param command to parse
// @return CmdValue correspondent
Skeleton::CmdValues Skeleton::cmdToEnum(string command) {
	map<string, CmdValues> cmd;
	cmd[":version"] = cmdVersion;
	cmd[":name"] = cmdName;
	cmd[":units"] = cmdUnits;
	cmd[":documentation"] = cmdDoc;
	cmd[":root"] = cmdRoot;
	cmd[":bonedata"] = cmdBoneData;
	cmd[":hierarchy"] = cmdHierarchy;
	return cmd[command];
}

// Read words in a line
// @param line a line for break in words
// @return vector of words in line
vector<string> Skeleton::readWords(string line) {
	string whitespace = " \n\t\r";

	if (line.size() == 0)
		return vector<string>();

	if (line[line.size() - 1] == '\\') {
		line[line.size() - 1] = ' ';
	}

	//split the line into words
	vector<string> words;
	string::size_type pos = 0;
	while (pos != string::npos) {
		pos = line.find_first_not_of(whitespace, pos);
		if (pos == string::npos)
			break;
		string::size_type eow = line.find_first_of(whitespace, pos);
		words.push_back(string(line, pos, eow - pos));
		pos = eow;
	}

	return words;
}

// Read an ASF skeleton file
// @param asfFile name of file
// @return read sucefully or not
bool Skeleton::loadASF(string asfFile) {

	ifstream file(asfFile.c_str());

	string line, current_command;
	vector<string> words;

	// for bones reading
	int id, current_limit;
	string name, dof, baxis_order;
	Vector3d direction, baxis;
	double length, bodymass, cofmass;
	Vector2d limit;
	vector<Vector2d> limits;

	id = current_limit = -1;
	name = dof = baxis_order = "";
	direction.x = direction.y = direction.z = 0;
	baxis.x = baxis.y = baxis.z = 0;
	length = bodymass = cofmass = -1.0;
	limits.clear();
	limit.x = limit.y = 0;

	// for hierarchy reading
	vector<string> sons;
	sons.clear();

	if (file.is_open()) {
		file >> noskipws;
		while (!file.eof()) {
			getline(file, line);
			words = readWords(line);
			if (line[0] == ':') {
				current_command = words[0];
			} else if (line[0] == '#' || line == "") {
				continue; // comentary line or end line
			}
			switch (cmdToEnum(current_command)) {

				case cmdVersion: // read version of skeleton
					version = line.substr(9);
					break;

				case cmdName: // read name of skeleton
					name = line.substr(6);
					break;

				case cmdUnits: // read units of skeleton
					words = readWords(line);
					if (words[0] == "mass") { // mass
						mass = strtod(words[1].c_str(), NULL);
					} else if (words[0] == "length") { // length
						length = strtod(words[1].c_str(), NULL);
					} else if (words[0] == "angle") { // angle
						is_degree = words[1] == "deg" ? true : false;
					}
					break;

				case cmdDoc: // read documentation of skeleton
					if (line[0] == ':') {
						doc = "";
					} else {
						for (unsigned int i = 0; i < words.size(); i ++)
							doc += words[i] + ' ';
						doc += '\n';
					}
					break;

				case cmdRoot: // read root of skeleton
					if (words[0] == "order") { // order
						order = "";
						for (unsigned int i = 1; i < words.size(); i++)
							if (words[i][0] == 'T' || words[i][0] == 't') {
								switch (words[i][1]) {
									case 'x':
									case 'X':
										order += 'x';
										break;
									case 'y':
									case 'Y':
										order += 'y';
										break;
									case 'z':
									case 'Z':
										order += 'z';
										break;
									default:
										break;
								}
							} else if (words[i][0] == 'R' || words[i][0] == 'r') {
								switch (words[i][1]) {
									case 'x':
									case 'X':
										order += 'X';
										break;
									case 'y':
									case 'Y':
										order += 'Y';
										break;
									case 'z':
									case 'Z':
										order += 'Z';
										break;
									default:
										break;
								}
							}
					} else if (words[0] == "axis") { // axis
						axis = words[1];
					} else if (words[0] == "position") { // position
						position.x = strtod(words[1].c_str(), NULL);
						position.y = strtod(words[2].c_str(), NULL);
						position.z = strtod(words[3].c_str(), NULL);
					} else if (words[0] == "orientation") { // orientation
						orientation.x = strtod(words[1].c_str(), NULL);
						orientation.y = strtod(words[2].c_str(), NULL);
						orientation.z = strtod(words[3].c_str(), NULL);
					}
					break;

				case cmdBoneData: // read bones of skeleton
					if (words[0] == "begin") { // begin [reseting values]
						id = current_limit = -1;
						name = dof = baxis_order = "";
						direction.x = direction.y = direction.z = 0;
						baxis.x = baxis.y = baxis.z = 0;
						length = bodymass = cofmass = -1.0;
						limits.clear();
						limit.x = limit.y = 0;
					} else if (words[0] == "id") { // read bone id
						id = (int) strtod(words[1].c_str(), NULL);
					} else if (words[0] == "name") {
						name = words[1];
					} else if (words[0] == "direction") { // read bone direction
						direction.x = strtod(words[1].c_str(), NULL);
						direction.y = strtod(words[2].c_str(), NULL);
						direction.z = strtod(words[3].c_str(), NULL);
					} else if (words[0] == "length") { // read bone length
						length = strtod(words[1].c_str(), NULL);
					} else if (words[0] == "axis") { // read bone axis
						baxis.x = strtod(words[1].c_str(), NULL);
						baxis.y = strtod(words[2].c_str(), NULL);
						baxis.z = strtod(words[3].c_str(), NULL);
						baxis_order = words[4];
					} else if (words[0] == "bodymass") { // read bone bodymass
						bodymass = strtod(words[1].c_str(), NULL);
					} else if (words[0] == "cofmass") { // read bone cofmass
						cofmass = strtod(words[1].c_str(), NULL);
					} else if (words[0] == "dof") { // read bone dof
						dof = "";
						for (unsigned int i = 1; i < words.size(); i++)
							if (words[i][0] == 'r') {
								switch (words[i][1]) {
									case 'x':
									case 'X':
										dof += 'X';
										break;
									case 'y':
									case 'Y':
										dof += 'Y';
										break;
									case 'z':
									case 'Z':
										dof += 'Z';
										break;
									default:
										break;
								}
							} else {
								switch (words[i][1]) {
									case 'x':
									case 'X':
										dof += 'x';
										break;
									case 'y':
									case 'Y':
										dof += 'y';
										break;
									case 'z':
									case 'Z':
										dof += 'z';
										break;
									default:
										break;
								}
							}
						current_limit = words.size() - 2;
					} else if (words[0] == "limits" || current_limit> 0) { // read bone limits
						limit.x = strtod(words[words.size() - 2].substr(1).c_str(), NULL);
						limit.y = strtod(words[words.size() - 1].substr(0, words[words.size() - 1].size() - 1).c_str(), NULL);
						limits.push_back(limit);
						current_limit -= 1;
					} else if (words[0] == "end") { // end [inserting on map]

						// create axis for this bone
						Vector3d axis_x = make_vector(1.0, 0.0, 0.0);
						Vector3d axis_y = make_vector(0.0, 1.0, 0.0);
						Vector3d axis_z = make_vector(0.0, 0.0, 1.0);
						
						// rotate axis
						for (int i = baxis_order.size() - 1; i >= 0; i--) {
							if (baxis_order[i] == 'X' && baxis.x != 0) {
								axis_y = rotate(baxis.x, axis_y, axis_x);
								axis_z = rotate(baxis.x, axis_z, axis_x);
							} else if (baxis_order[i] == 'Y' && baxis.y != 0) {
								axis_x = rotate(baxis.y, axis_x, axis_y);
								axis_z = rotate(baxis.y, axis_z, axis_y);								
							} else if (baxis_order[i] == 'Z' && baxis.z != 0) {
								axis_x = rotate(baxis.z, axis_x, axis_z);
								axis_y = rotate(baxis.z, axis_y, axis_z);								
							}
						}
						
						Bone bone(id, name, direction, length, baxis, baxis_order, bodymass, cofmass, dof, limits,
								axis_x, axis_y, axis_z);

						bones[name] = bone;
					}
					break;

				case cmdHierarchy: // read hierarchy of skeleton
					if (words[0] == "begin") { // begin [reseting values]
						sons.clear();
						name = "";
					} else if (words[0] == "end") { // end [inserting on map]
						// finish hierarchy read
					} else if (words[0][0] != ':') { // insert one dependence on map
						name = words[0];
						for (unsigned int i = 1; i < words.size(); i++)
							sons.push_back(words[i]);
						hierarchy[name] = sons;
						sons.clear();
						name = "";
					}
					break;

				default:
					break;

			}
		}
		file.close();
	} else {
		cout << "Problem on open file" << endl;
		return false;
	}
	return true;
}

// Read an AMC motion data file.
// @param amcFile name of file
// @return read sucefully or not
bool Skeleton::loadAMC(string amcFile) {

	ifstream file(amcFile.c_str());

	string line;
	vector<string> words;

	string current_bone;
	int current_frame = 0;
	Vector3d current_position;
	map<string, Bone> current_skeleton;
	map<string, Vector3d> rotations;

	if (file.is_open()) {
		file >> noskipws;
		while (!file.eof()) {
			getline(file, line);
			words = readWords(line);
			if (line[0] == ':' || line[0] == '#') { // comentary line or initial line
				continue;
			} else if (line == "" || words.size() == 1) { // initial frame line
				if (line != "")
					current_frame = (int) strtod(words[0].c_str(), NULL);
				else
					current_frame++;
				if (current_frame > 1) {
					//for(map<string, Vector3d>::reverse_iterator it = rotations.rbegin(); it != rotations.rend(); it++) {
					for (map<string, Vector3d>::iterator it = rotations.begin(); it != rotations.end(); it++) {

						current_bone = it->first;

						Vector3d rot;
						double alfa = it->second.x, beta = it->second.y, gama = it->second.z;
						string str_dof;

						Vector3d axis_x = make_vector(1.0, 0.0, 0.0);
						Vector3d axis_y = make_vector(0.0, 1.0, 0.0);
						Vector3d axis_z = make_vector(0.0, 0.0, 1.0);

						if (current_bone == "root") {
							str_dof = order;
						} else {
							axis_x = current_skeleton[current_bone].getAxis_X();
							axis_y = current_skeleton[current_bone].getAxis_Y();
							axis_z = current_skeleton[current_bone].getAxis_Z();
							str_dof = current_skeleton[current_bone].getDOF();
						}

						list<string> queue;
						queue.push_back(current_bone);
						vector<string> current_sons;

						while (!queue.empty()) {
							current_bone = queue.front();
							queue.pop_front();

							if (current_bone != "root") {
								rot = current_skeleton[current_bone].getDirection();
								current_skeleton[current_bone].setAngles(alfa, beta, gama);
								
								for (int i = 0; i < (int) str_dof.size(); i++) {
									if(str_dof[i] == 'X' and alfa != 0) {
										rot = rotate(alfa, rot, axis_x);
										current_skeleton[current_bone].setAxis_X(rotate(alfa, current_skeleton[current_bone].getAxis_X(), axis_x));
										current_skeleton[current_bone].setAxis_Y(rotate(alfa, current_skeleton[current_bone].getAxis_Y(), axis_x));
										current_skeleton[current_bone].setAxis_Z(rotate(alfa, current_skeleton[current_bone].getAxis_Z(), axis_x));
									} else if(str_dof[i] == 'Y' and beta != 0) {
										rot = rotate(beta, rot, axis_y);
										current_skeleton[current_bone].setAxis_X(rotate(beta, current_skeleton[current_bone].getAxis_X(), axis_y));
										current_skeleton[current_bone].setAxis_Y(rotate(beta, current_skeleton[current_bone].getAxis_Y(), axis_y));
										current_skeleton[current_bone].setAxis_Z(rotate(beta, current_skeleton[current_bone].getAxis_Z(), axis_y));
									} else if(str_dof[i] == 'Z' and gama != 0) {
										rot = rotate(gama, rot, axis_z);
										current_skeleton[current_bone].setAxis_X(rotate(gama, current_skeleton[current_bone].getAxis_X(), axis_z));
										current_skeleton[current_bone].setAxis_Y(rotate(gama, current_skeleton[current_bone].getAxis_Y(), axis_z));
										current_skeleton[current_bone].setAxis_Z(rotate(gama, current_skeleton[current_bone].getAxis_Z(), axis_z));
									}
								}
								current_skeleton[current_bone].setDirection(rot);
							}

							// Refreshing queue
							current_sons = hierarchy[current_bone];
							for (int i = 0; i < (int) current_sons.size(); i++) {
								queue.push_back(current_sons[i]);
							}
						}
					}

					Pose pose = buildPose(current_position, current_skeleton);
					skeletons.push_back(current_skeleton);
					motion.push_back(pose);
				}

				current_skeleton = bones;
				continue;
			} else {
				current_bone = words[0];

				double alfa, beta, gama;
				alfa = beta = gama = 0;
				string str_dof;

				if (current_bone == "root") {
					str_dof = order;
				} else {
					str_dof = current_skeleton[current_bone].getDOF();
				}

				for (unsigned int i = 1; i < words.size(); i++) {
					double desl = strtod(words[i].c_str(), NULL);
					switch (str_dof[i - 1]) {
						case 'x':
							current_position.x = desl;
							break;
						case 'y':
							current_position.y = desl;
							break;
						case 'z':
							current_position.z = desl;
							break;
						case 'X':
							alfa = desl;
							break;
						case 'Y':
							beta = desl;
							break;
						case 'Z':
							gama = desl;
							break;
					}
				}

				rotations[current_bone] = make_vector(alfa, beta, gama);
			}
		}
		file.close();
	} else {
		cout << "Problem on open file" << endl;
		return false;
	}
	return true;
}

// Read a BVH file
// @param bvhFile name of file
// @return read sucefully or not
bool Skeleton::loadBVH(string bvhFile) {

	ifstream file(bvhFile.c_str());

	string line;
	//, current_command;
	vector<string> words;
	//
	//    // for bones reading
	//    int id, current_limit;
	//    string name, dof, baxis_order;
	//    Vector3d direction, baxis;
	//    double length, bodymass, cofmass;
	//    Vector2d limit;
	//    vector<Vector2d> limits;
	//
	//    id = current_limit = -1;
	//    name = dof = baxis_order = "";
	//    direction.x = direction.y = direction.z = 0;
	//    baxis.x = baxis.y = baxis.z = 0;
	//    length = bodymass = cofmass = -1.0;
	//    limits.clear();
	//    limit.x = limit.y = 0;
	//
	//    // for hierarchy reading
	//    vector<string> sons;
	//    sons.clear();

	if (file.is_open()) {
		cout << "abriu..." << endl;
		file >> noskipws;
		cout << "lendo..." << endl;
		while (!file.eof()) {
			getline(file, line);
			words = readWords(line);
			for (int i = 0; i < words.size(); i++) {
				cout << "[" << words[i] << "], ";
			}
			cout << endl;
		}
		cout << "leu..." << endl;
	}

}

// Export animation on pss format.
// @param skelInFile given skel input file
// @param pssFile name of file
// @return export sucefully or not
bool Skeleton::exportPSS(string skelInFile, string pssFile) {

	vector<Vector3d> oSkel;
	ifstream fskelinput(skelInFile.c_str());
	//assert(fskelinput);
	int nj;
	fskelinput.ignore(numeric_limits<int>::max(), '\n');
	fskelinput >> nj;
	int id, idant;
	for (int i = 0; i < nj; i++) {
		Vector3d e;
		fskelinput >> id >> e.x >> e.y >> e.z >> idant;
		oSkel.push_back(e);
	}

	float scale = 1.0/40.0;

	cout << "exporting PSS file..." << endl;
	ofstream file(pssFile.c_str());
	file << "POSES_SKEL" << endl;
	file << motion.size() << " 18" << endl;
	file << "0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17" << endl;

	file << (joints[0]["thorax"] * scale) - oSkel[0];
	file << (joints[0]["upperback"] * scale) - oSkel[1];
	file << (joints[0]["root"] * scale) - oSkel[2];
	file << (joints[0]["head"] * scale) - oSkel[3];
	file << (joints[0]["rhipjoint"] * scale) - oSkel[4];
	file << (joints[0]["rfemur"] * scale) - oSkel[5];
	file << (joints[0]["rtibia"] * scale) - oSkel[6];
	file << (joints[0]["rtoes"] * scale) - oSkel[7];
	file << (joints[0]["lhipjoint"] * scale) - oSkel[8];
	file << (joints[0]["lfemur"] * scale) - oSkel[9];
	file << (joints[0]["ltibia"] * scale) - oSkel[10];
	file << (joints[0]["ltoes"] * scale) - oSkel[11];
	file << (joints[0]["rclavicle"] * scale) - oSkel[12];
	file << (joints[0]["rhumerus"] * scale) - oSkel[13];
	file << (joints[0]["rhand"] * scale) - oSkel[14];
	file << (joints[0]["lclavicle"] * scale) - oSkel[15];
	file << (joints[0]["lhumerus"] * scale) - oSkel[16];
	file << (joints[0]["lhand"] * scale) - oSkel[17];

	Joints jt_ant;
	for (int i = 1; i < (int) joints.size(); i++) {
		jt_ant = joints[i - 1];
		file << (joints[i]["thorax"] - jt_ant["thorax"]) * scale;
		file << (joints[i]["upperback"] - jt_ant["upperback"]) * scale;
		file << (joints[i]["root"] - jt_ant["root"]) * scale;
		file << (joints[i]["head"] - jt_ant["head"]) * scale;
		file << (joints[i]["rhipjoint"] - jt_ant["rhipjoint"]) * scale;
		file << (joints[i]["rfemur"] - jt_ant["rfemur"]) * scale;
		file << (joints[i]["rtibia"] - jt_ant["rtibia"]) * scale;
		file << (joints[i]["rtoes"] - jt_ant["rtoes"]) * scale;
		file << (joints[i]["lhipjoint"] - jt_ant["lhipjoint"]) * scale;
		file << (joints[i]["lfemur"] - jt_ant["lfemur"]) * scale;
		file << (joints[i]["ltibia"] - jt_ant["ltibia"]) * scale;
		file << (joints[i]["ltoes"] - jt_ant["ltoes"]) * scale;
		file << (joints[i]["rclavicle"] - jt_ant["rclavicle"]) * scale;
		file << (joints[i]["rhumerus"] - jt_ant["rhumerus"]) * scale;
		file << (joints[i]["rhand"] - jt_ant["rhand"]) * scale;
		file << (joints[i]["lclavicle"] - jt_ant["lclavicle"]) * scale;
		file << (joints[i]["lhumerus"] - jt_ant["lhumerus"]) * scale;
		file << (joints[i]["lhand"] - jt_ant["lhand"]) * scale;
		file << endl;
	}
	file.close();
	cout << "PSS file exported sucefully!" << endl;
	return true;
}

#endif
